/*********************************************************************
 * Software License Agreement (Modified BSD License)
 *
 * Copyright (c) 2009-2010, Willow Garage, Daniel Munoz
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *   * Neither the name of the copyright holders' organizations nor the
 *     names of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

#include <m3n/random_field.h>

using namespace std;

// --------------------------------------------------------------
/* See function definition */
// --------------------------------------------------------------
int RandomField::saveNodeFeatures(string filename) const
{
  ofstream file_out(filename.c_str());
  if (file_out.is_open() == false)
  {
    cerr << "RandomField::saveNodeFeatures Could not save to: " << filename << endl;
    return -1;
  }

  file_out << "# File format: x y z node_id label nbr_features [features]" << endl;

  for (map<unsigned int, RandomField::Node*>::const_iterator iter_nodes = m_nodes.begin() ; iter_nodes
      != m_nodes.end() ; iter_nodes++)
  {
    const RandomField::Node* curr_node = iter_nodes->second;

    // x y z node_id label nbr_features
    file_out << curr_node->getX() << " " << curr_node->getY() << " " << curr_node->getZ() << " "
        << curr_node->getID() << " " << curr_node->getLabel() << " "
        << curr_node->getNumberFeatureVals();

    // [features]
    const vector<double>& curr_feats = curr_node->getFeatureVals();
    for (unsigned int feature_idx = 0 ; feature_idx < curr_node->getNumberFeatureVals() ; feature_idx++)
    {
      // TODO set precision
      file_out << " " << curr_feats[feature_idx];
    }
    file_out << endl;
  }
  file_out.close();
  return 0;
}

// --------------------------------------------------------------
/* See function definition */
// --------------------------------------------------------------
int RandomField::saveCliqueFeatures(string basename) const
{
  const unsigned int nbr_clique_sets = m_clique_sets.size();
  for (unsigned int cs_idx = 0 ; cs_idx < nbr_clique_sets ; cs_idx++)
  {
    // Generate filename for current clique set's features
    stringstream ss_curr_filename;
    ss_curr_filename << basename << ".cs_" << cs_idx << "_features";
    string curr_filename = ss_curr_filename.str();

    // Open file
    ofstream file_out(curr_filename.c_str());
    if (file_out.is_open() == false)
    {
      cerr << "RandomField::saveCliqueFeatures Could not save to: " << curr_filename << endl;
      return -1;
    }

    file_out << "# File format: x y z clique_set_idx clique_id nbr_features [features]" << endl;

    // Write to file: x y z clique_set_idx clique_id nbr_features [features]
    const map<unsigned int, Clique*>& cliques = m_clique_sets[cs_idx];
    for (map<unsigned int, Clique*>::const_iterator iter_cliques = cliques.begin() ; iter_cliques
        != cliques.end() ; iter_cliques++)
    {
      const unsigned int curr_clique_id = iter_cliques->first;
      const RandomField::Clique* curr_clique = iter_cliques->second;

      // x y z clique_set_idx clique_id nbr_features
      file_out << curr_clique->getX() << " " << curr_clique->getY() << " " << curr_clique->getZ()
          << " " << cs_idx << " " << curr_clique_id << " " << curr_clique->getNumberFeatureVals();

      // [features]
      const vector<double>& curr_feats = curr_clique->getFeatureVals();
      for (unsigned int feature_idx = 0 ; feature_idx < curr_clique->getNumberFeatureVals() ; feature_idx++)
      {
        // TODO set precision
        file_out << " " << curr_feats[feature_idx];
      }
      file_out << endl;
    }

    file_out.close();
  }
  return 0;
}

// --------------------------------------------------------------
/* See function definition */
// --------------------------------------------------------------
int RandomField::saveRandomFieldWGASCII(string basename) const
{
  string rf_fname = basename;
  rf_fname.append(".random_field");
  ofstream file_out(rf_fname.c_str());
  if (file_out.is_open() == false)
  {
    cerr << "RandomField::saveRandomFieldWGASCII Could not save to: " << rf_fname << endl;
    return -1;
  }

  string node_features_fname = basename;
  node_features_fname.append(".node_features");
  if (saveNodeFeatures(node_features_fname) < 0)
  {
    file_out.close();
    return -1;
  }

  if (saveCliqueFeatures(basename) < 0)
  {
    file_out.close();
    return -1;
  }

  file_out << "# File format:" << endl;
  file_out << "# node_features_filename" << endl;
  file_out << "# " << endl;
  file_out << "# S=nbr_clique_sets" << endl;
  file_out << "# clique_set_0_features_filename" << endl;
  file_out << "# 0 P=nbr_cliques_in_set" << endl;
  file_out << "# 0 clique_id_0 nbr_nodes_in_clique [node ids]" << endl;
  file_out << "#    ..." << endl;
  file_out << "# 0 clique_id_P nbr_nodes_in_clique [node ids]" << endl;
  file_out << "#" << endl;
  file_out << "# ..." << endl;
  file_out << "# clique_set_(S-1)_features_filename" << endl;
  file_out << "# (S-1) P=nbr_cliques" << endl;
  file_out << "# (S-1) clique_id_0 nbr_nodes_in_clique [node ids]" << endl;
  file_out << "#    ..." << endl;
  file_out << "# (S-1) clique_id_P nbr_nodes_in_clique [node ids]" << endl;

  file_out << node_features_fname << endl;
  file_out << endl;
  file_out << m_clique_sets.size() << endl;
  for (unsigned int i = 0 ; i < m_clique_sets.size() ; i++)
  {
    const map<unsigned int, Clique*>& cliques = m_clique_sets[i];

    // print clique-set feature filename
    stringstream ss_cs_fname;
    ss_cs_fname << basename << ".cs_" << i << "_features";
    file_out << ss_cs_fname.str() << endl;

    // clique-set-idx and number of cliques
    file_out << i << " " << cliques.size() << endl;

    // print each clique in the clique set
    for (map<unsigned int, Clique*>::const_iterator iter_cliques = cliques.begin() ; iter_cliques
        != cliques.end() ; iter_cliques++)
    {
      const list<unsigned int>& node_ids = iter_cliques->second->getNodeIDs();
      file_out << i << " " << iter_cliques->first << " " << node_ids.size();

      // print node ids in the clique
      for (list<unsigned int>::const_iterator iter_node_ids = node_ids.begin() ; iter_node_ids
          != node_ids.end() ; iter_node_ids++)
      {
        file_out << " " << *iter_node_ids;
      }
      file_out << endl;
    }

    file_out << endl;
  }

  file_out.close();
  return 0;
}

// --------------------------------------------------------------
/* See function definition */
// --------------------------------------------------------------
int RandomField::loadRandomFieldWGASCII(string basename)
{
  string rf_fname = basename;
  rf_fname.append(".random_field");
  ifstream file_rf(rf_fname.c_str());
  if (file_rf.is_open() == false)
  {
    cerr << "RandomField::loadRandomFieldWGASCII Could not load RF: " << rf_fname << endl;
    return -1;
  }

  clear();

  // skip header (hack)
  for (int i = 0 ; i < 16 ; i++)
  {
    file_rf.ignore(std::numeric_limits<int>::max(), '\n');
  }

  // ------------------------------------------
  // Load node features file
  // format: x y z node_id label nbr_features [features]
  char node_features_fname[512];
  file_rf.getline(node_features_fname, 512);
  ifstream file_node_features(node_features_fname);
  if (file_node_features.is_open() == false)
  {
    cerr << "RandomField::loadRandomFieldWGASCII Could not load nodes: " << node_features_fname
        << endl;
    file_rf.close();
    return -1;
  }
  // skip header
  file_node_features.ignore(std::numeric_limits<int>::max(), '\n');
  string line;
  while (getline(file_node_features, line))
  {
    double x = 0.0;
    double y = 0.0;
    double z = 0.0;
    unsigned int node_id = 0;
    unsigned int node_label = RandomField::UNKNOWN_LABEL;
    unsigned int nbr_feature_vals = 0;

    istringstream iss(line);
    iss >> x;
    iss >> y;
    iss >> z;
    iss >> node_id;
    iss >> node_label;
    iss >> nbr_feature_vals;

    vector<double> feature_vals(nbr_feature_vals, 0.0);
    for (unsigned int i = 0 ; i < nbr_feature_vals ; i++)
    {
      iss >> feature_vals[i];
    }

    if (createNode(node_id, feature_vals, node_label, x, y, z) == NULL)
    {
      abort();
    }
  }
  file_node_features.close();

  // ------------------------------------------
  // Load clique set info
  unsigned int nbr_clique_sets = 0;
  file_rf >> nbr_clique_sets;
  file_rf.ignore(std::numeric_limits<int>::max(), '\n'); // finish the line

  // For each clique-set, read its features and clique<->node membership info
  m_clique_sets.resize(nbr_clique_sets);
  m_clique_set_dims.assign(nbr_clique_sets, 0);
  for (unsigned int cs_idx = 0 ; cs_idx < nbr_clique_sets ; cs_idx++)
  {
    map<unsigned int, Clique> tempo_cliques; // clique feautres for current clique set

    // --------------------------
    // Load clique features
    // format: x y z clique_set_idx clique_id nbr_features [features]
    char clique_features_fname[512];
    file_rf.getline(clique_features_fname, 512);
    ifstream file_clique_features(clique_features_fname);
    if (file_clique_features.is_open() == false)
    {
      cerr << "RandomField::loadRandomFieldWGASCII Could not load cliques: " << clique_features_fname
          << endl;
      file_rf.close();
      return -1;
    }

    // skip header
    file_clique_features.ignore(std::numeric_limits<int>::max(), '\n');
    string line;
    while (getline(file_clique_features, line))
    {
      double x = 0.0;
      double y = 0.0;
      double z = 0.0;
      unsigned int read_cs_idx = 0;
      unsigned int clique_id = 0;
      unsigned int nbr_feature_vals = 0;

      istringstream iss(line);
      iss >> x;
      iss >> y;
      iss >> z;
      iss >> read_cs_idx;
      iss >> clique_id;
      iss >> nbr_feature_vals;
      if (cs_idx != read_cs_idx)
      {
        abort();
      }

      vector<double> feature_vals(nbr_feature_vals, 0.0);
      for (unsigned int i = 0 ; i < nbr_feature_vals ; i++)
      {
        iss >> feature_vals[i];
      }

      Clique new_clique(clique_id);
      new_clique.setXYZ(x, y, z);
      new_clique.setFeatures(feature_vals);
      tempo_cliques[clique_id] = new_clique;
    }
    file_clique_features.close();

    // --------------------------
    // Read node membership for each clique
    // Format:
    //   cs_idx_i P=nbr_cliques_in_set
    //     ... loop over j < P...
    //   cs_idx_i clique_j_id nbr_nodes_in_clique [node ids]
    unsigned int read_cs_idx = 0;
    unsigned int read_nbr_cliques = 0;
    file_rf >> read_cs_idx;
    file_rf >> read_nbr_cliques;

    // verify reading the clique for the correct clique-set
    if (cs_idx != read_cs_idx)
    {
      abort();
    }
    // verify the read number of cliques matches what was read from feature file
    if (tempo_cliques.size() != read_nbr_cliques)
    {
      abort();
    }

    // Iterate over each clique in the clique-set
    for (unsigned int clique_index = 0 ; clique_index < tempo_cliques.size() ; clique_index++)
    {
      unsigned int read_cs_idx2 = 0;
      unsigned int clique_id = 0;
      unsigned int clique_order = 0;

      file_rf >> read_cs_idx2;
      file_rf >> clique_id;
      file_rf >> clique_order;

      // verify the read clique-set index and clique id exist
      if (cs_idx != read_cs_idx2)
      {
        abort();
      }

      // Retrieve tempo clique
      if (tempo_cliques.count(clique_id) == 0)
      {
        abort();
      }
      Clique& curr_clique = tempo_cliques[clique_id];

      // read the node ids contained in each clique
      list<const Node*> c_nodes;
      for (unsigned int node_index = 0 ; node_index < clique_order ; node_index++)
      {
        unsigned int node_id = 0;
        file_rf >> node_id;

        // verify it exists
        if (m_nodes.count(node_id) == 0)
        {
          abort();
        }
        c_nodes.push_back(m_nodes[node_id]);
      }

      // Now create the actual clique in the random field
      if (createClique(curr_clique.getID(), cs_idx, c_nodes, curr_clique.getFeatureVals(),
                       curr_clique.getX(), curr_clique.getY(), curr_clique.getZ()) == NULL)
        abort();
    }

    file_rf.ignore(std::numeric_limits<int>::max(), '\n'); // finish the line
    file_rf.ignore(std::numeric_limits<int>::max(), '\n'); // skip blank line
  }
  file_rf.close();
  return 0;
}
